/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc2856.robot;

/**
 *
 * @author Dustin
 */

import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.SpeedController;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class UltimateAscentBot extends IterativeRobot {
    UserInterface ui;
    HolonomicDrive holoDrive;
    Shooter launcher;
    Aiming aimer;
    Autonomous autonomous;
    Camera camera;
    Climber climb;
    int autoCounter = 0;
   // TCPNetworking networking;
    //Climber climber
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        
        Jaguar hdJag0 = new Jaguar(RobotMap.dt_jagFL_mod, RobotMap.dt_jagFL_chan);
        Jaguar hdJag1 = new Jaguar(RobotMap.dt_jagBL_mod, RobotMap.dt_jagBL_chan);
        Jaguar hdJag2 = new Jaguar(RobotMap.dt_jagFR_mod, RobotMap.dt_jagFR_chan);
        Jaguar hdJag3 = new Jaguar(RobotMap.dt_jagBR_mod, RobotMap.dt_jagBR_chan);
        
        Gyro gyro = new Gyro(RobotMap.dt_gyro_mod, RobotMap.dt_gyro_chan);
        gyro.setSensitivity(RobotMap.dt_gyro_sensitivity);
        holoDrive = new HolonomicDrive(hdJag0, hdJag1, hdJag2, hdJag3, gyro);
        holoDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
        holoDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        
        launcher = new Shooter(false);
        //Climber climb = new Climber();
        //stager = new Staging();
        Solenoid aimerSolA = new Solenoid(RobotMap.aim_solA_mod, RobotMap.aim_solA_chan);
        Solenoid aimerSolB = new Solenoid(RobotMap.aim_solB_mod, RobotMap.aim_solB_chan);
        aimer = new Aiming(aimerSolA, aimerSolB, holoDrive);
        
        Compressor comp = new Compressor(1, 1);
        comp.start();
       
        /*Relay cameraRelay = new Relay(RobotMap.cam_relay_mod, RobotMap.cam_relay_chan);
        Servo cameraServo = new Servo(RobotMap.cam_servo_mod, RobotMap.cam_servo_chan);
        camera = new Camera(cameraServo, cameraRelay);*/
        
        ui = new UserInterface(holoDrive, launcher, climb, aimer);
        
        autonomous = new Autonomous(holoDrive, launcher);
        
        
        
        //networking = new TCPNetworking(RobotMap.server_port);
    }

    public void autonomousInit() {
        //launcher.spinStop();
        //autonomous.x = 0;
        autoCounter = 0;
        launcher.spinEffort(0);
    }
    
    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        //autonomous.command(RobotMap.auto_Commands);
        autoCounter++;
        //Resolution is ~50 counts/second; 
        switch(autoCounter){
            case 400: //8s
                launcher.spinEffort(0.75);
                break;
            case 500: //10s
                launcher.shoot(true);
                break;
            case 525: //10.5s
                launcher.shoot(false);
                break;
            case 550: //11.5s
                launcher.shoot(true);
                break;
            case 600: //12s
                launcher.shoot(false);
                launcher.spinEffort(0);
                break;
            default:
                break;
        }
        
    }
    
    public void disabledInit() {
        System.out.println("autocounter: "+autoCounter);
    }

    public void teleopInit() {
        launcher.spinStop();
       
    }
    
    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        ui.update();
    }

}
